using System;
using System.Collections;
using System.Drawing;
using FIRADriverLibrary;
using Geometry;



namespace PotentialFieldDriver
{
	public abstract class PotencialDriver
	{
		public static ArrayList angleLocks;
		public static Point2D checkPoint;

		public const double wheelsHalfDistance = 0.034;
		public const double robotCollisionAvoidanceDistance = 0.2;

		public static double maxVelocity = 0.6;
		public static double tinyVelocity = 0.1;	//below robot will change direction if needed, above will turn
		public static double freeCurveFactor = 0.6;
		public static double collisionDetectionTime = 0.5;		
		
		//public static double collisionAvoidanceFactor = 2;

		public static FIRADriver firaDriver;
		public static RectangleF pitchArea;

		public static void Drive (RobotController robotController, double x, double y, double eta)
		{
			FIRARobotState robot = robotController.robotState;
			double linearVel = (robot.currentLeftSpeed + robot.currentRightSpeed) / 2;
			double deltaVel = (robot.currentLeftSpeed - robot.currentRightSpeed) / 2;

			checkPoint = CalculateCollisionFreeCheckPoint(robot, x, y, Math.Abs(collisionDetectionTime * linearVel) + robotCollisionAvoidanceDistance);
			linearVel = (robot.currentLeftSpeed + robot.currentRightSpeed) / 2;//might have chaged
			deltaVel = (robot.currentLeftSpeed - robot.currentRightSpeed) / 2;
			if (checkPoint.x != 0.0 || checkPoint.y != 0.0)
			{
				double toCheckPointDistance = Vector2D.GetLength(robot.x - checkPoint.x, robot.y - checkPoint.y);
				double fromCheckPointDistance = Vector2D.GetLength(x - checkPoint.x, y - checkPoint.y);

				CalculateControlNoObstacles(robot, checkPoint.x, checkPoint.y, eta * (toCheckPointDistance / (toCheckPointDistance+fromCheckPointDistance))
					, ref linearVel, ref deltaVel);
			}
			else
			{
				CalculateControlNoObstacles(robot, x, y, eta, ref linearVel, ref deltaVel);
				angleLocks.Clear();
			}

			robot.leftSpeed = linearVel + deltaVel;
			robot.rightSpeed = linearVel - deltaVel;
		}


		public static void DriveBlind(RobotController robotController, double x, double y, double eta)
		{
			FIRARobotState robot = robotController.robotState;
			double linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;
			double deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;
			CalculateControlNoObstacles(robot, x, y, eta, ref linearVel, ref deltaVel);
			robot.leftSpeed = linearVel + deltaVel;
			robot.rightSpeed = linearVel - deltaVel;
		}


		private static void CalculateControlNoObstacles(FIRARobotState robot, double x, double y, double eta, ref double linearVel, ref double deltaVel)
		{
			Vector2D robotVersor = new Vector2D(robot.angle);
			Vector2D targetVector = new Vector2D(x-robot.x, y-robot.y);

            double angleRobotTarget = Vector2D.AngleBetween(robotVersor, targetVector);

			linearVel = Math.Sign(Math.Cos(angleRobotTarget)) *
				 Math.Min(maxVelocity, targetVector.Length / eta);
			deltaVel = freeCurveFactor * Math.Sin(angleRobotTarget) * linearVel;

			if (targetVector.Length < robotCollisionAvoidanceDistance)	// sharpen turn if close to target
			{
				deltaVel *= 1 + (robotCollisionAvoidanceDistance - targetVector.Length) / robotCollisionAvoidanceDistance;
			}

			if (Math.Abs(robot.leftSpeed + robot.rightSpeed) / 2 > tinyVelocity)	// robot at motion
			{
				if (Math.Sign(robot.leftSpeed + robot.rightSpeed) != Math.Sign(linearVel))	// invert motion
				{
					linearVel = -linearVel;
					deltaVel = Math.Sign(Math.Sin(angleRobotTarget)) * freeCurveFactor * linearVel;
				}
			}
		}

		public class AngleLock
		{
			public Vector2D lockVector;
			public double halfAngle;

			public AngleLock (double lockCenterAngle, double halfAngle, double distance)
			{
				this.lockVector = new Vector2D(lockCenterAngle);
				this.lockVector *= distance;
				this.halfAngle = halfAngle;
			}

			public bool TryJoin(AngleLock secondLock)
			{
				double angleBetween = Vector2D.AngleBetween(lockVector, secondLock.lockVector);

				if (Math.Abs(angleBetween) > halfAngle + secondLock.halfAngle)
					return false;

				/// the second range can exceed the first from one or both sides
				double wider_big = Math.Abs(angleBetween) + secondLock.halfAngle - halfAngle;
				double wider_small = - Math.Abs(angleBetween) + secondLock.halfAngle - halfAngle;
				if (wider_big > 0)
				{
					if (wider_small < 0)	
						wider_small = 0;
					double length = Math.Min(lockVector.Length, secondLock.lockVector.Length);
					lockVector = new Vector2D(lockVector.Angle + Math.Sign(angleBetween) * (wider_big - wider_small) / 2);
					this.lockVector *= length;
					this.halfAngle += (wider_big + wider_small) / 2;
					this.halfAngle = Math.Min(this.halfAngle, Math.PI);
				}
				return true;
			}

			public double GetClosestFreeAngle(double angle)
			{
				Vector2D robotVersor = new Vector2D(angle);
				double angleBetween = Vector2D.AngleBetween(lockVector, robotVersor);
				if (Math.Abs(angleBetween) > this.halfAngle)
					return angle;

				return lockVector.Angle + Math.Sign(angleBetween) * this.halfAngle;
			}
		}
		
		private static Point2D CalculateCollisionFreeCheckPoint(FIRARobotState robot, double x, double y, double lookupDistance)
		{
//			Vector2D robotVector = new Vector2D(robot.angle);
//			double robotAngle = robot.angle * Math.Sign(robot.leftSpeed + robot.rightSpeed);

			Vector2D targetVector = new Vector2D(x-robot.x, y-robot.y);
			Vector2D robotVector = targetVector + (new Vector2D(robot.angle) * (lookupDistance  * Math.Sign(robot.leftSpeed + robot.rightSpeed)));
			double robotAngle = robotVector.Angle;

			angleLocks = new ArrayList();
			Vector2D robotObstacleRobotVector;
            		
			// robots locks
			foreach (FIRARobotState obstacleRobot in firaDriver.firaRobots)
			{
				if (obstacleRobot == robot)
					continue;

				robotObstacleRobotVector.X = obstacleRobot.x - robot.x;
				robotObstacleRobotVector.Y = obstacleRobot.y - robot.y;
				double obstacleDistance = robotObstacleRobotVector.Length;
				if (obstacleDistance > lookupDistance)
					continue;

				double obstacleAngle = robotObstacleRobotVector.Angle;
				double obstacleWidthHalfAngle = Math.Atan(robotCollisionAvoidanceDistance / obstacleDistance);
				if (obstacleDistance < robotCollisionAvoidanceDistance)
				obstacleWidthHalfAngle *= 1 + (robotCollisionAvoidanceDistance - obstacleDistance) / robotCollisionAvoidanceDistance;
				angleLocks.Add(new AngleLock(obstacleAngle, obstacleWidthHalfAngle, obstacleDistance));
			}

			//walls locks
			double wallDistance = Math.Max(0.01,robot.x - pitchArea.Left);
			if (wallDistance > 0.0 && wallDistance < lookupDistance)
			{				
				angleLocks.Add(new AngleLock(Math.PI, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}
			wallDistance = Math.Max(0.01,pitchArea.Right - robot.x);
			if (wallDistance > 0.0 && wallDistance < lookupDistance)
			{		
				angleLocks.Add(new AngleLock(0.01, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}
			wallDistance = Math.Max(0.01,robot.y - pitchArea.Top);
			if (wallDistance < lookupDistance)
			{				
				angleLocks.Add(new AngleLock(3*Math.PI/2, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}
			wallDistance = Math.Max(0.01,pitchArea.Bottom - robot.y);
			if (wallDistance < lookupDistance)
			{		
				angleLocks.Add(new AngleLock(Math.PI/2, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}

			//remove too far locks, stop if a very close lock is present
			for (int i = 0 ; i < angleLocks.Count ; i++)
			{
				double obstacleDistance = ((AngleLock)angleLocks[i]).lockVector.Length;
				if (obstacleDistance > targetVector.Length)	 
				{
					angleLocks.RemoveAt(i);
					i--;
				}
				else if (obstacleDistance < robotCollisionAvoidanceDistance /2)
				{
					robot.leftSpeed = 0.0;
					robot.rightSpeed = 0.0;
				}
			}

			///join locks
			for (int i = 0 ; i < angleLocks.Count ; i++)
			{
				AngleLock angleLock = (AngleLock)angleLocks[i];
				if (angleLock.lockVector.Length > targetVector.Length)	 
				{
					angleLocks.RemoveAt(i);
					i--;
				}
				for (int j = i + 1 ; j < angleLocks.Count ; j++)
				{
					if (angleLock.TryJoin((AngleLock)angleLocks[j]))
					{
						angleLocks.RemoveAt(j);
						j--;
					}
				}
			}

			// find and go around
			double newAngle = robotAngle;
			foreach (AngleLock angleLock in angleLocks)
			{
					
				newAngle = angleLock.GetClosestFreeAngle(newAngle);
				if (newAngle != robotAngle)
				{
					if (angleLock.lockVector.Length < robotCollisionAvoidanceDistance) //prevent keeping motion direction
					{						
						robot.leftSpeed = 0.0;
						robot.rightSpeed = 0.0;
					}
					return new Point2D(robot.x + angleLock.lockVector.Length * Math.Cos(newAngle),
						robot.y + angleLock.lockVector.Length * Math.Sin(newAngle));
				}
			}
			return new Point2D(0.0, 0.0);

		}












		// simple, good for separated obsacles
//		private static void Avoid(FIRARobotState robot, ref double linearVel, ref double deltaVel)
//		{
//			Vector2D robotVector = new Vector2D(robot.angle) * linearVel * collisionDetectionTime;
//			double robotVectorLength = robotVector.Length;
//			Vector2D robotObstacleRobotVector;
//
//			double influence = 0.0;
//
//			foreach (FIRARobotState obstacleRobot in firaDriver.firaRobots)
//			{
//				if (obstacleRobot == robot)
//					continue;
//
//				robotObstacleRobotVector.X = obstacleRobot.x - robot.x;
//				robotObstacleRobotVector.Y = obstacleRobot.y - robot.y;
//
//				double angleRobotRobotObstacle = Vector2D.AngleBetween(robotVector, robotObstacleRobotVector);
//
//				if (Math.Cos(angleRobotRobotObstacle) < 0)
//					continue;
//
//				if (robotObstacleRobotVector.Length / Math.Cos(angleRobotRobotObstacle) > robotVectorLength)
//					continue;
//
//				influence += (robotVectorLength - robotObstacleRobotVector.Length) * Math.Sign(Math.Sin(angleRobotRobotObstacle)) * Math.Cos(angleRobotRobotObstacle);
//			}
//
//
//			deltaVel -= collisionAvoidanceFactor *(influence / robotVectorLength);
//		}

	}
}
